#!/usr/bin/env python
# -*- coding:utf-8 -*-
# @FileName  :q4.py
# @Time      :2024/9/8 3:59
# @Author    :YKW
import math

import matplotlib.pyplot as plt
import numpy as np
from scipy.integrate import quad
from shapely.geometry import MultiPolygon
from shapely.geometry import Polygon
import pandas as pd
import numpy as np
from scipy.optimize import fsolve
import random
import matplotlib.pyplot as plt
from shapely.geometry import Point, LineString


# ToDo body更新逻辑修改


def secant_method(f, x0, x1, now, tolerance=1e-6, max_iterations=1000):
    """
    画线法求now在下一时刻的目标值
    x0=now+0.001
    x1=now+0.3
    """
    for i in range(max_iterations):
        fx0 = f(x0, now)  # 1.65 - (x0极角和now极角对应两个把手的直线距离)
        fx1 = f(x1, now)
        if fx1 - fx0 == 0:  # 如果
            return x1
        denominator = fx1 - fx0
        x2 = x1 - fx1 * ((x1 - x0) / denominator)
        if abs(x2 - x1) < tolerance:
            return x2
        x0, x1 = x1, x2
    return x2


def binary_search(f, a, b, now, tol=1e-6, max_iter=1000):
    if f(a, now) * f(b, now) >= 0:
        raise ValueError("f(a) 和 f(b) 必须异号")

    for _ in range(max_iter):
        mid = (a + b) / 2
        f_mid = f(mid, now)
        if abs(f_mid) < tol:
            return mid
        elif f(a, now) * f_mid < 0:
            b = mid
        else:
            a = mid
    print("answer not find")
    return None


def polar_distance(theta1, theta2):
    """
    返回两个极角对应点的的直线距离
    """
    r1 = b * theta1
    r2 = b * theta2
    x1 = r1 * np.cos(theta1)
    y1 = r1 * np.sin(theta1)
    x2 = r2 * np.cos(theta2)
    y2 = r2 * np.sin(theta2)
    if theta1 < 0:
        y1 = -y1
    if theta2 < 0:
        y2 = -y2
    distance = math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
    # 计算角度差
    angle_diff = theta2 - theta1
    # 计算距离
    # distance = math.sqrt((r1 - r2 * math.cos(angle_diff)) ** 2 + (r2 * math.sin(angle_diff)) ** 2)
    return distance


def cal_body(theta, now):
    length = 1.65
    distance = polar_distance(theta, now)
    return distance - length


def cal_head(theta, now):
    length = 2.86
    distance = polar_distance(theta, now)
    return distance - length


def get_theta_position(now):  # 给龙头，给全部
    result = [now]
    num = 223 - 1 - 1
    # body = binary_search(cal_head, now + 0.001, now + 1, now)
    body = secant_method(cal_head, now + 0.001, now + 0.3, now)
    result.append(body)
    for i in range(num):
        # next = binary_search(cal_body, body + 0.001, body + 1, body)
        next = secant_method(cal_body, body + 0.001, body + 0.3, body)
        result.append(next)
        body = next
    return result


def integrand(theta):  # 掉头区域，大圆形小圆相切
    # TODO
    if theta > theta_p1:
        return np.sqrt((b * theta) ** 2 + b ** 2)
    elif theta < theta_p2:
        return -np.sqrt((b * theta) ** 2 + b ** 2)
    elif theta_p2 < theta < 0:
        return -r_arc
    else:
        return 2 * r_arc


def find_theta_1(theta_1, theta_0, length=0.01):  # length=v_head*timespace
    length = timespace * speed
    return quad(integrand, theta_1, theta_0)[0] - length


def get_theta_time(now, time_now, time_next, timespace):  # 给一点，算时序
    result = [now]
    t = 0
    while abs(t - (time_next - time_now)) > 0.01 * timespace:
        t += timespace
        # theta_prev = result[-1]
        # theta_new = fsolve(find_theta_1, theta_prev - 1, args=(theta_prev))[0]
        theta_new = secant_method(find_theta_1, now + 0.001, now + 0.3, now)
        result.append(theta_new)
        now = theta_new
    return result


def get_init_position(now, time, timespace):
    result = [now]
    t = 0
    # mul = int(1 / timespace)
    # idx = [i * mul for i in range(int(time))]
    # idx=[0]
    # count=0
    while abs(t - time) > 0.01 * timespace:
        t += timespace
        # count+=1
        # if abs(t-int(t))<0.01:
        #     idx.append(count)
        # theta_new = fsolve(find_theta_1, theta_prev - 1, args=(theta_prev))[0]
        theta_new = secant_method(find_theta_1, now + 0.001, now + 0.3, now)
        result.append(theta_new)
        now = theta_new
    # if len(idx)>301:
    #     idx=idx[:301]
    # result = [result[i] for i in idx]
    # print(len(result))

    return result


def cal_total():
    head = get_theta_time(theta_0_1, 0, 300, timespace)
    ans = []
    for i in head:
        tem = get_theta_position(i)
        ans.append(tem)
    return ans


def cal_velocity_components(theta_a, theta_b, v_a):
    """计算速度分解到ab线段方向的分量"""
    # 计算螺线在a点和b点的切线角度
    r1 = b * theta_a
    r2 = b * theta_b
    x_0 = r1 * np.cos(theta_a)
    y_0 = r1 * np.sin(theta_a)
    x_b = r2 * np.cos(theta_b)
    y_b = r2 * np.sin(theta_b)
    tangent_angle_a = theta_a + np.pi / 2
    tangent_angle_b = theta_b + np.pi / 2

    # 计算ab线段的角度
    ab_angle = np.arctan2(y_b - y_0, x_b - x_0)

    # 计算速度分解到ab线段方向的分量
    v_a_line = v_a * np.cos(tangent_angle_a - ab_angle)
    v_b_line = v_a_line
    v_b = v_b_line / np.cos(tangent_angle_b - ab_angle)
    return v_b


def cal_line(point1, point2):
    x1, y1 = point1
    x2, y2 = point2

    A = y2 - y1
    B = x1 - x2

    # 计算 C
    C = - (A * x1 + B * y1)

    return (A, B, C)


def cal_points_on_line(point1, point2, length):
    l = length
    x1, y1 = point1
    x2, y2 = point2
    if x1 < x2:
        x1, x2 = x2, x1
        y1, y2 = y2, y1
    elif x1 == x2 and y1 < y2:
        x1, x2 = x2, x1
        y1, y2 = y2, y1
    # 计算直线的方向向量
    direction = [x2 - x1, y2 - y1]

    # 计算方向向量的模长
    direction_length = np.sqrt(direction[0] ** 2 + direction[1] ** 2)

    # 标准化方向向量
    direction_unit = [direction[0] / direction_length, direction[1] / direction_length]

    # 计算两个方向上的点
    if direction_unit[0] < 0 or (direction_unit[0] == 0 and direction_unit[1] < 0):
        direction_unit[0], direction_unit[1] = -direction_unit[0], -direction_unit[1]
    point1_plus = [x1 + direction_unit[0] * l, y1 + direction_unit[1] * l]
    point1_minus = [x2 - direction_unit[0] * l, y2 - direction_unit[1] * l]

    return point1_plus, point1_minus


def cal_vertical_dot(point1, point2, length):
    l = length
    x1, y1 = point1
    x2, y2 = point2
    direction = [x2 - x1, y2 - y1]
    direction_length = np.sqrt(direction[0] ** 2 + direction[1] ** 2)
    direction_unit = [direction[0] / direction_length, direction[1] / direction_length]
    direction_unit = [-direction_unit[1], direction_unit[0]]
    if direction_unit[0] < 0 or (direction_unit[0] == 0 and direction_unit[1] < 0):
        direction_unit[0], direction_unit[1] = -direction_unit[0], -direction_unit[1]
    point1_plus = [x1 + direction_unit[0] * l, y1 + direction_unit[1] * l]
    point1_minus = [x2 - direction_unit[0] * l, y2 - direction_unit[1] * l]
    return point1_plus, point1_minus


class node:
    p = 0.55  # 螺距，单位：米
    b = p / (2 * np.pi)  # b的值

    def __init__(self, time, position, speed=None, theta=None):
        self.length = 2.86 if position == 0 else 1.65
        self.type = 0 if position == 0 else 1
        self.theta = theta
        self.r = None
        self.x = None
        self.y = None
        self.speed = speed
        self.speed_x = None
        self.speed_y = None
        self.time = time
        self.before = None
        self.after = None
        self.box = None

    def refresh(self):
        if self.theta is None:
            print("ERROR NO Theta")
            return 0
        self.r = b * self.theta
        self.x = self.r * np.cos(self.theta)
        self.y = self.r * np.sin(self.theta)
        if self.before is not None:
            self.speed = cal_velocity_components(self.before.theta, self.theta, self.before.speed)


class bench:
    def __init__(self, node1, node2):
        node1.refresh()
        node2.refresh()
        p1 = (node1.x, node1.y)
        p2 = (node2.x, node2.y)
        mid1, mid2 = cal_points_on_line(p1, p2, length=0.275)
        self.dotA, self.dotB = cal_vertical_dot(mid1, mid2, length=0.15)
        self.dotC, self.dotD = cal_vertical_dot(mid2, mid1, length=0.15)
        shell = [self.dotA, self.dotD, self.dotB, self.dotC]
        self.box = Polygon(shell)


class Dragon:
    start_theat = 16 * 2 * np.pi

    def __init__(self, time=0, timespace=1, body_num=222, head_speed=1):
        self.time = time
        self.timespace = timespace
        time_list = get_init_position(self.start_theat, self.time, self.timespace)
        self.head = node(time, 0, speed=head_speed, theta=time_list[-1] if time else self.start_theat)
        pos_list = get_theta_position(time_list[-1]) if time else [self.start_theat for i in range(1, body_num + 2)]
        self.length = body_num + 1
        self.body = [node(time, position, theta=pos_list[position])
                     for position in range(1, body_num + 1)]

        self.head.after = self.body[0]
        self.body[0].after = self.body[1]
        for i in range(1, len(self.body) - 1):
            self.body[i].before = self.body[i - 1]
            self.body[i].after = self.body[i + 1]
        self.body[0].before = self.head
        self.body[-1].before = self.body[-2]
        self.state = 1
        self.history = []
        self.crash_time = []

        self.alpha=0  # 掉头区域圆心角
        self.stage=0  # 0盘入，1掉头，2盘出

    def __func_time__(self, theta):  # 写成分段函数
        return np.sqrt((b * theta) ** 2 + b ** 2)
        # if not self.crash_time:
        #     return np.sqrt((b * theta) ** 2 + b ** 2)
        # elif self.time<self.crash_time:
        #     return np.sqrt((b * theta) ** 2 + b ** 2)
        # else:
        #     pass

    def __func_position__(self, theta):
        pass

    def __node_refresh__(self):
        self.head.refresh()
        for i in self.body:
            i.refresh()

    def check_state(self):  # 1 if 未翻转 else 0
        self.state = 0

    def __loop_time__(self, theta, target):
        length = self.head.speed * self.timespace
        return quad(self.__func_time__, theta, target)[0] - length
        # return quad(integrand, theta, target)[0] - length

    def __loop_position__(self, theta, target):
        pass

    def __loop_out__(self, theta, target):
        pass

    # def secant_method(f, x0, x1, now, tolerance=1e-6, max_iterations=1000):
    #     for i in range(max_iterations):
    #         fx0 = f(x0, now)
    #         fx1 = f(x1, now)
    #         if fx1 - fx0 == 0:
    #             return x1
    #         denominator = fx1 - fx0
    #         x2 = x1 - fx1 * ((x1 - x0) / denominator)
    #         if abs(x2 - x1) < tolerance:
    #             return x2
    #         x0, x1 = x1, x2
    #     return x2

    def __get_theta_time__(self, time):
        result = [self.head.theta]
        time_sque = [(self.head.theta, 0)]
        t = 0
        now = self.head.theta
        while abs(t - time) > 0.01 * self.timespace:
            t += self.timespace
            theta_new = secant_method(self.__loop_time__, now + 0.001, now + 0.3, now)
            result.append(theta_new)
            now = theta_new
            time_sque.append((theta_new, t + self.time))
        for i in time_sque:
            self.history.append((i))
        return result

    def __move_body__(self):
        pos_list = get_theta_position(self.head.theta)
        for i in range(len(self.body)):
            self.body[i].theta = pos_list[i + 1]
            self.body[i].refresh()

    def __move_head__(self, time):
        time_list = self.__get_theta_time__(time)
        self.time = time + self.time
        self.head.theta = time_list[-1]
        self.head.refresh()

    def move(self, time_more):
        self.__move_head__(time_more)
        self.__move_body__()
        self.__node_refresh__()
        self.__box_refresh__()

    def move_to(self, time):
        delta = time - self.time
        if delta <= 0:
            raise ValueError("试图回到过去")
        self.__move_head__(delta)
        self.__move_body__()

    def move_forward(self):
        if self.state:
            pass

    def __box_refresh__(self):
        self.head.box = bench(self.head, self.head.after).box
        for i in self.body:
            if i.after is not None:
                i.box = bench(i, i.after).box

    def check_crash(self):
        """
        检测是否为碰撞状态
        """
        self.__box_refresh__()
        # print(self.time)
        boxes = [self.head.box]
        # boxes=boxes[:220]
        for i in self.body:
            if i.box is not None:
                boxes.append(i.box)

        for i in range(2, len(boxes)):
            flag = boxes[0].intersects(boxes[i])
            if flag:
                self.crash_time.append(self.time)
                return (0, i)
        return -1, -1

        # for i in range(len(boxes)):
        #     for j in range(len(boxes)):
        #         if abs(i - j) <= 1:
        #             continue
        #         flag = boxes[i].intersects(boxes[j])
        #         if flag:
        #             return (i, j)
        # return -1, -1

    def draw(self):
        self.__box_refresh__()
        boxes = [self.head.box]
        # boxes=boxes[:220]
        for i in self.body:
            if i.box is not None:
                boxes.append(i.box)
        multipolygon = MultiPolygon(boxes)
        # 创建一个图形和轴
        fig, ax = plt.subplots()

        # 遍历 MultiPolygon 中的每个 Polygon 并绘制它们
        for polygon in multipolygon.geoms:
            x, y = polygon.exterior.xy
            ax.plot(x, y)

        # 设置坐标轴的比例相同
        ax.set_aspect('equal')

        # 显示图形
        plt.show()

    def draw_head(self):
        self.__box_refresh__()
        fig, ax = plt.subplots()
        x, y = self.head.box.exterior.xy
        ax.plot(x, y)
        ax.set_aspect('equal')
        plt.show()


def transpose_2d(data):
    transposed = list(map(list, zip(*data)))
    return transposed


"""loop.py"""


def draw_loop(b=0.1, total=16 * 2 * np.pi, a=0):
    theta1 = np.linspace(0, total, 1000)
    theta2 = np.linspace(0, total, 1000)

    r1 = a + b * theta1
    r2 = a + b * theta2

    # 将极坐标转换为笛卡尔坐标
    x1 = r1 * np.cos(theta1)
    y1 = r1 * np.sin(theta1)
    # x2 = r2 * np.cos(theta2)
    # y2 = r2 * np.sin(theta2)
    x2 = -x1
    y2 = -y1
    # x2=x1
    # y2=y1
    x = np.concatenate((x1, x2))
    y = np.concatenate((y1, y2))
    # 创建图形和轴
    fig, ax = plt.subplots()

    # 绘制阿基米德螺线
    ax.plot(x, y, 'b-', lw=2)
    ax.set_title('Archimedean Spiral')
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.grid(True)
    ax.set_aspect('equal')  # 确保x和y轴的比例相同

    # 显示图形
    plt.show()
    plt.close()


class cycle:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.r = None

    def cal_distance(self, x, y):
        """
        其他点到圆心距离
        """
        return np.sqrt((x - self.x) ** 2 + (y - self.y) ** 2)

    def fun_find(self, x, y, r):
        return self.cal_distance(x, y) - r


def dt(a, b):
    """
    向量点积
    """
    return a[0] * b[0] + a[1] * b[1]


class loop:
    """
        模拟或计算涉及循环路径的物理系统
        通过笛卡尔坐标和极坐标之间的转换，以及相关的几何计算，来模拟和处理循环路径上点的位置和运动。
    """

    def __init__(self, a=0, b=1.7, tol=0.01):
        self.a = a
        self.b = b  # 螺距
        self.r = None  # 极坐标半径
        self.theta = None  # 极坐标极角
        self.x = None  # 笛卡尔坐标系 x
        self.y = None  # 笛卡尔坐标系 y
        self.state = 0  # 0:no change 1:x,y 2:r,theta
        self.tol = tol  # 容忍度
        self.vec_xy = None  # 笛卡尔坐标系 到原点 倾斜角
        self.vec_rc = None  # None
        self.rc = None
        # self.n_r=None
        self.n_x = None  # 法线向量
        self.dot = None  # Point类，几何位置
        self.t_x = None  # 切线向量
        self.t_r = None  # None
        self.t_n = None  # None
        self.alpha = None

    def refresh(self):
        """
        根据state,更新参数
        """
        if self.state == 0:
            # print("NO change took place")
            pass
        if self.state == 2:
            self.x = self.r * np.cos(self.theta)
            self.y = self.r * np.sin(self.theta)
        elif self.state == 1:
            self.r = np.sqrt(self.x ** 2 + self.y ** 2)
            self.theta = self.r / self.b
        # self.n_r = (-np.sin(self.theta), np.cos(self.theta))
        self.n_x = np.array(((np.sin(self.theta) + self.theta * np.cos(self.theta)) / (np.cos(self.theta) -
                                                                                       self.theta * np.sin(self.theta)),
                             -1))
        # l=np.sqrt(self.n_r[0]**2+self.n_r[1]**2)
        # self.n_r=(self.n_r[0]/l,self.n_r[1]/l)
        l = np.sqrt(self.n_x[0] ** 2 + self.n_x[1] ** 2)
        self.n_x = np.array((self.n_x[0] / l, self.n_x[1] / l))  # 标准化 单位向量
        self.state = 0
        self.dot = Point(self.x, self.y)
        test_dot = Point(self.x + self.n_x[0], self.y + self.n_x[1])
        O = Point(0, 0)
        if test_dot.distance(O) > self.dot.distance(O):
            self.n_x = np.array((-self.n_x[0], -self.n_x[1]))
        self.t_x = np.array((self.n_x[1], -self.n_x[0]))
        if self.x * self.t_x[0] + self.y * self.t_x[1] < 0:  # it can proof that (x,y)*t_x is always positive
            self.t_x = np.array((-self.t_x[0], -self.t_x[1]))
        self.vec_xy = np.array((self.x, self.y))

    def get(self, m, n, state=1):
        """
        极坐标和笛卡尔坐标系互转
        """
        if state == 1:
            self.x = m
            self.y = n
            self.state = 1
            self.refresh()
        elif state == 2:

            self.theta = n
            self.r = self.b * n
            self.state = 2
            self.refresh()
        else:
            raise ValueError("state err")

    def check(self):
        """
        判断是否正确设置参数：1.极坐标和笛卡尔正确映射 2.r 和 b*theta相等
        """
        self.refresh()
        if abs(np.tan(self.theta) - self.y / self.x) > self.tol or abs(self.r - self.b * self.theta) > self.tol:
            raise ValueError("Not on loop")

    def cycle(self, rc):
        pass


class combined:
    """
    组合前后两个圆弧，方便计算
    """

    def __init__(self, d1, d2):
        d1.refresh()
        d2.refresh()
        self.dot1 = d1
        self.dot2 = d2

    # 返回掉头路径的长度
    def get(self, tol=0.001):  # experiment changing dot
        """
        只知道螺线和圆弧切点的时候，通过二分确定圆弧圆弧相切时的半径
        """
        # line0=LineString([self.dot1.dot,self.dot2.dot])
        rl = 0
        rr = 15  # min and max,change it if bug emerged
        while rl < rr:
            mid = (rl + rr) / 2
            r = mid
            # dot1是大圆 圆心坐标，要和self.dot1区分
            dot1 = Point(self.dot1.x + self.dot1.n_x[0] * r * 2, self.dot1.y + self.dot1.n_x[1] * r * 2)
            # line1=LineString([self.dot1.dot,tem])
            dot2 = Point(self.dot2.x + self.dot2.n_x[0] * r, self.dot2.y + self.dot2.n_x[1] * r)
            # line2=LineString([self.dot2.dot,tem])
            # dotc=line1.intersection(line2)
            # assert dotc.geo_type == "Point"
            # 满足圆弧段相切的条件
            if abs(dot1.distance(dot2) - 3 * r) <= tol:
                rl = rr = mid
                break
            if dot1.distance(dot2) - 3 * r > tol:
                rl = mid
            else:
                rr = mid

        self.dot1.rc = 2 * rl
        self.dot2.rc = rl

        vec = np.array((dot1.x - dot2.x, dot1.y - dot2.y))
        lvec = np.sqrt(vec[0] ** 2 + vec[1] ** 2)
        vec = (vec[0] / lvec, vec[1] / lvec)  # 单位化方向向量
        alpha1 = dt(vec, self.dot1.n_x)
        alpha2 = dt(vec, self.dot2.n_x)
        alpha1 = np.arccos(alpha1)  #
        alpha2 = np.arccos(alpha2)
        vec1 = -self.dot1.t_x
        if np.dot(self.dot1.t_x, vec) > 0:
            alpha1 = 2 * np.pi - alpha1
        if np.dot(self.dot2.t_x, vec) > 0:
            alpha2 = 2 * np.pi - alpha2

        self.dot1.alpha = alpha1  # 大圆圆心角
        self.dot2.alpha = alpha2  # 小圆圆心角

        return alpha1 * 2 * rl + alpha2 * rl  # alp1*rc1+alp2*rc2


def generate(theta, b=1.7 / (2 * np.pi)):
    r = theta * b
    x = r * np.cos(theta)
    y = r * np.sin(theta)
    return np.array((x, y))


# 目标函数
def objective_function(x):
    return x.get()

def init_solution(theta):
    global p, b
    loop1 = loop()
    loop2 = loop()
    p1 = generate(theta=theta)
    p2 = np.flipud(generate(theta=theta))  # 盘出路线切点
    loop1.get(p1[0], p1[1])
    loop2.get(p2[0], p2[1])
    com = combined(loop1, loop2)
    return com


p = 1.7
b = p / (2 * np.pi)  # b的值
r_0_1 = p * 16  # 初始半径 r_0^(1)
theta_0_1 = 16 * 2 * np.pi  # 初始角度 theta_0^(1)
timespace = 1
speed = 1
if __name__ == "__main__":
    theta_p1 = 0
    theta_p2 = 0
    r_arc = 0
    st_times = 1300
    check = -1
    for theta in range(20, 28):  # r/b
        theta_p1 = theta
        theta_p2 = -theta
        r_arc = 1 * b * theta / 2
        dragon = Dragon(time=st_times, timespace=timespace)
        initial_solution = init_solution(theta)
        times = st_times

        while check == -1 and times < 1400:
            head = dragon.head
            times += timespace
            dragon.move_to(times)
            check, a = dragon.check_crash()
            print(times, head.theta)
            if check != -1:
                print("!!!")

        dragon.draw()

    # while check == -1 and times < 2000:
    #     head = dragon.head
    #     times += timespace
    #     dragon.move_to(times)
    #     #check, a = dragon.check_crash()
    #     print(times,head.theta)
    #     if check != -1:
    #         print("!!!")
    #
    #     if head == 0:
    #         print(p, times)
    #         continue
    # theta = head.theta
    # r = b * theta
    # r = abs(r)
    # print(r, theta, p, times)
    # dragon.draw()

# timespace = 0.01
# times = 400
# dragon = Dragon(time=times, timespace=timespace)
# tem = []
# teem = []
# count = 0
# eval_fre = 10
# # for times in range(290, 350):
# #     dragon.time_check(times)
# #     dragon.draw()
# #     print(dragon.head.theta)
#
# while times < 450:
#     print(times)
#     times += timespace
#     count += 1
#     # if count%eval_fre==0:
#     #     dragon.draw()
#     dragon.move(timespace)
#     i, j = dragon.check_crash()
#     # dragon.draw()
#     if i != -1:
#         # dragon.draw()
#         print("Crash!!!")
#         print(i, j)
#         tem.append((i, j))
#         teem.append(times)
#         break
#     else:
#         print(dragon.head.theta)

# dragon=Dragon(timespace=0.1)
# times=10
# for times in range(1,300):
#     dragon.move_to(times)
#     print(dragon.head.theta,dragon.time)
# print(1)
# time=300
# timespace=0.1
# dragon1=Dragon(time,timespace=0.1)
# for time in range(301,450):
#     dragon1.move_to(time)

# p = 1.7
# b = p / (2 * np.pi)
# draw_loop(b)
# dragon = Dragon(timespace=0.1)
# times = 0
# fig, ax = plt.subplots()
# x, y = [], []
# for times in range(1, 3000):
#     dragon.move_to(times)
#     x.append(dragon.head.x)
#     y.append(dragon.head.y)
#
#     # dragon.draw_head()
# ax.plot(x, y)
# plt.show()
